#include "DinoMath.h"

namespace DinoMath
{
	void MakeProjectionMatrix(float *m, float fovy, float aspect, float znear, float zfar)
	{
		float ymax, xmax;
		float temp, temp2, temp3, temp4;
		ymax = znear * tanf(fovy * M_PI / 360.0);
		//ymin = -ymax;
		//xmin = -ymax * aspectRatio;
		xmax = ymax * aspect;
		glhFrustumf2(m, -xmax, xmax, -ymax, ymax, znear, zfar);
	}

	void glhFrustumf2(float *matrix, float left, float right, float bottom, float top,
					  float znear, float zfar)
	{
		float temp, temp2, temp3, temp4;
		temp = 2.0 * znear;
		temp2 = right - left;
		temp3 = top - bottom;
		temp4 = zfar - znear;
		matrix[0] = temp / temp2;
		matrix[1] = 0.0;
		matrix[2] = 0.0;
		matrix[3] = 0.0;
		matrix[4] = 0.0;
		matrix[5] = temp / temp3;
		matrix[6] = 0.0;
		matrix[7] = 0.0;
		matrix[8] = (right + left) / temp2;
		matrix[9] = (top + bottom) / temp3;
		matrix[10] = (-zfar - znear) / temp4;
		matrix[11] = -1.0;
		matrix[12] = 0.0;
		matrix[13] = 0.0;
		matrix[14] = (-temp * zfar) / temp4;
		matrix[15] = 0.0;
	}


	void glhPerspectivef2(float *matrix, float fovyInDegrees, float aspectRatio,
						  float znear, float zfar)
	{
		float ymax, xmax;
		float temp, temp2, temp3, temp4;
		ymax = znear * tanf(fovyInDegrees * M_PI / 360.0);
		//ymin = -ymax;
		//xmin = -ymax * aspectRatio;
		xmax = ymax * aspectRatio;
		glhFrustumf2(matrix, -xmax, xmax, -ymax, ymax, znear, zfar);
	}


	/*inline*/ void CopyVector3(Vector3 dst, const Vector3 src) { memcpy(dst, src, sizeof(Vector3)); }

	//=========================================================================================================================
	#define A(row,col)  a[(col<<2)+row]
	#define B(row,col)  b[(col<<2)+row]
	#define P(row,col)  product[(col<<2)+row]

	/*inline*/ void MatrixMultiply44(Matrix44 product, const Matrix44 a, const Matrix44 b )
	{
		for (int i = 0; i < 4; i++) {
			float ai0=A(i,0),  ai1=A(i,1),  ai2=A(i,2),  ai3=A(i,3);
			P(i,0) = ai0 * B(0,0) + ai1 * B(1,0) + ai2 * B(2,0) + ai3 * B(3,0);
			P(i,1) = ai0 * B(0,1) + ai1 * B(1,1) + ai2 * B(2,1) + ai3 * B(3,1);
			P(i,2) = ai0 * B(0,2) + ai1 * B(1,2) + ai2 * B(2,2) + ai3 * B(3,2);
			P(i,3) = ai0 * B(0,3) + ai1 * B(1,3) + ai2 * B(2,3) + ai3 * B(3,3);
		}
	}

	#undef A
	#undef B
	#undef P

	//=========================================================================================================================
	/*inline */void LoadIdentity44(Matrix44 m)
	{
		// Don't be fooled, this is still column major
		static Matrix44	identity = { 1.0f, 0.0f, 0.0f, 0.0f,
			0.0f, 1.0f, 0.0f, 0.0f,
			0.0f, 0.0f, 1.0f, 0.0f,
			0.0f, 0.0f, 0.0f, 1.0f };

		memcpy(m, identity, sizeof(Matrix44));
	}

	//=========================================================================================================================
	/*inline*/ void TranslationMatrix44(Matrix44 m, float x, float y, float z)
	{ 
		LoadIdentity44(m); m[12] = x; m[13] = y; m[14] = z; 
	}

	//=========================================================================================================================


	inline void RotationMatrix44(Matrix44 m, float angle, float x, float y, float z)
	{
		float mag, s, c;
		float xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;

		s = float(sin(angle));
		c = float(cos(angle));

		mag = float(sqrt( x*x + y*y + z*z ));

		// Identity matrix
		if (mag == 0.0f) {
			LoadIdentity44(m);
			return;
		}

		// Rotation matrix is normalized
		x /= mag;
		y /= mag;
		z /= mag;

		#define M(row,col)  m[col*4+row]

		xx = x * x;
		yy = y * y;
		zz = z * z;
		xy = x * y;
		yz = y * z;
		zx = z * x;
		xs = x * s;
		ys = y * s;
		zs = z * s;
		one_c = 1.0f - c;

		M(0,0) = (one_c * xx) + c;
		M(0,1) = (one_c * xy) - zs;
		M(0,2) = (one_c * zx) + ys;
		M(0,3) = 0.0f;

		M(1,0) = (one_c * xy) + zs;
		M(1,1) = (one_c * yy) + c;
		M(1,2) = (one_c * yz) - xs;
		M(1,3) = 0.0f;

		M(2,0) = (one_c * zx) - ys;
		M(2,1) = (one_c * yz) + xs;
		M(2,2) = (one_c * zz) + c;
		M(2,3) = 0.0f;

		M(3,0) = 0.0f;
		M(3,1) = 0.0f;
		M(3,2) = 0.0f;
		M(3,3) = 1.0f;

		#undef M
	}



	/*__inline*/ void TransformVector3(Vector3 vOut, const Vector3 v, const Matrix44 m)
	{
		vOut[0] = m[0] * v[0] + m[4] * v[1] + m[8] *  v[2] + m[12];// * v[3];	// Assuming 1 
		vOut[1] = m[1] * v[0] + m[5] * v[1] + m[9] *  v[2] + m[13];// * v[3];	
		vOut[2] = m[2] * v[0] + m[6] * v[1] + m[10] * v[2] + m[14];// * v[3];	
		//vOut[3] = m[3] * v[0] + m[7] * v[1] + m[11] * v[2] + m[15] * v[3];
	}

	__inline void RotateVector(Vector3 vOut, const Vector3 p, const M3DMatrix33f m)
	{
	vOut[0] = m[0] * p[0] + m[3] * p[1] + m[6] * p[2];	
	vOut[1] = m[1] * p[0] + m[4] * p[1] + m[7] * p[2];	
	vOut[2] = m[2] * p[0] + m[5] * p[1] + m[8] * p[2];	
	}

	inline void ScaleVector3(Vector3 v, const float scale) { v[0] *= scale; v[1] *= scale; v[2] *= scale; }
	
	inline float GetVectorLengthSquared3(const Vector3 u) { return (u[0] * u[0]) + (u[1] * u[1]) + (u[2] * u[2]); }

	inline float GetVectorLength3(const Vector3 u) { return sqrtf(GetVectorLengthSquared3(u)); }

	/*inline*/  void NormalizeVector3(Vector3 u) { ScaleVector3(u, 1.0f / GetVectorLength3(u)); }

	inline void xNormalizeVector3(Vector3 v) { 
		float magnitude = sqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
		v[0] /= magnitude;
		v[1] /= magnitude;
		v[2] /= magnitude;
	}


	
	inline float DotProduct3(const Vector3 u, const Vector3 v) { return u[0]*v[0] + u[1]*v[1] + u[2]*v[2]; }

	/*inline*/ void CrossProduct3(Vector3 result, const Vector3 u, const Vector3 v)
	{
		result[0] = u[1]*v[2] - v[1]*u[2];
		result[1] = -u[0]*v[2] + v[0]*u[2];
		result[2] = u[0]*v[1] - v[0]*u[1];
	}

	inline void QuaternionMultScalar(Vector4 res, Vector4 q1, Vector4 q2)
	{
		res[0] = q1[0] * q2[0];
		res[1] = q1[1] * q2[1];
		res[2] = q1[2] * q2[2];
		res[3] = q1[3] * q2[3];
	}

	inline void QuaternionMultCross(Vector4 res, Vector4 q1, Vector4 q2)
	{
		Vector3 hlp, v1, v2;
		v1[0] = q2[0] * q1[3];
		v1[1] = q2[1] * q1[3];
		v1[2] = q2[2] * q1[3];

		v2[0] = q1[0] * q2[3];
		v2[1] = q1[1] * q2[3];
		v2[2] = q1[2] * q2[3];

		//CrossProduct3(hlp, q1, q2) + v2 * q1[3] + v1 * q2[3];

		CrossProduct3(hlp, q1, q2);

		res[0] = hlp[0] + v1[0] + v2[0];
		res[1] = hlp[1] + v1[1] + v2[1];
		res[2] = hlp[2] + v1[2] + v2[2];
		res[3] = q1[3] * q2[3] + DotProduct3(q1, q2);

	}

	inline float QuaternionNorm(Vector4 q)
	{
		return q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
	}

	inline void QuaternionToMatrix(Vector4 q, Matrix44 m)
	{
		
		LoadIdentity44(m);
		/*
		if(q[0] == 0.0f && q[1] == 0.0f && q[2] == 0.0f && q[3] == 0.0f)
		{
			return;
		}*/

		float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

		float s  = 2.0f/QuaternionNorm(q);  // 4 mul 3 add 1 div

		x2 = q[0] * s;    y2 = q[1] * s;    z2 = q[2] * s;
		xx = q[0] * x2;   xy = q[0] * y2;   xz = q[0] * z2;
		yy = q[1] * y2;   yz = q[1] * z2;   zz = q[2] * z2;
		wx = q[3] * x2;   wy = q[3] * y2;   wz = q[3] * z2;

		m[0] = 1.0f - (yy + zz);
		m[1] = xy + wz;
		m[2] = xz - wy;
		m[4] = xy - wz;
		m[5] = 1.0f - (xx + zz);
		m[6] = yz + wx;
		m[8] = xz + wy;
		m[9] = yz - wx;
		m[10] = 1.0f - (xx + yy);
	}

	float DistanceVec3(Vector3 vec1, Vector3 vec2)
	{
		float x, y, z;

		x = vec1[0] - vec2[0];
		y = vec1[1] - vec2[1];
		z = vec1[2] - vec2[2];

		return sqrtf(x*x + y*y + z*z);
	}
}

